/*
 * VenusGPS.cpp
 *
 *  Created on: Nov 28, 2011
 *      Author: mat
 */

#include <WProgram.h>
#include "VenusGPS.h"
#include "global.h"

uint8_t* payload = (uint8_t*)malloc(MAX_PKTLEN);

uint8_t VenusGPS::calcCRC(uint8_t type, uint8_t* payload, uint16_t len){
 uint8_t crc = 0, n;

 crc ^= type;

 for(n=0; n<len; n++){
	 crc ^=payload[n];
 }

 return crc;
}

void VenusGPS::send(uint8_t type, uint8_t payload[], uint16_t len){
	int n;
	uint8_t crc;

	crc = calcCRC(type, payload, len);

	Serial.write(START1);
	Serial.write(START2);
	Serial.write(len & 0xff);
	Serial.write(type);

	Serial.write(payload, len);

	Serial.write(crc);
	Serial.write(END1);
	Serial.write(END2);
	return;
}


void VenusGPS::init(){
	m_restart_t restart;
	uint16_t len = sizeof(m_restart_t);
	memset(&restart, 0, len);
	restart.start_mode = 0x03; //0x03 for cold restart
	payload = (uint8_t *)&restart;
	send(M_RESTART, payload, len);
}


uint8_t VenusGPS::proccess(uint8_t* inBuff){

int havepkt = 0, attemptCount=0;
uint8_t c;
static uint8_t state=START2, lncnt = 0, type;
static uint16_t len, n;

	while(Serial.available()>0 && attemptCount<MAX_GPSATTEMPS){
		c = Serial.read();
	 switch (state) {
	        case S_RESET:
	                /* cold reset state, look for the end of three lines */
	                if (c == 0x0a) {
	                 lncnt++;
	                 if (lncnt == 3) {
	                  state = S_POSTRESET; /* we have gotten past the inital reset burst */
	                 }
	                }
	                break;
	        case S_POSTRESET:
	                /* we are now into normal packet processing */
	                if (c == START1) {
	                 state = S_START1; /* start of packet */
	                }
	                /* otherwise, we just discard */
	                break;
	        case S_START1:
	                /* second marker of packet */
	                if (c == START2) {
	                 state = S_START2; /* here comes the length */
	                } else {
	                 state = S_POSTRESET; /* discard mode again */
	                }
	                break;
	        case S_START2:
	                /* the first low-byte part of the length */
	                len = c;
	                state = S_LENGTH1;
	                break;
	        case S_LENGTH1:
	                /* second byte is upper part */
	                len |= (c << 8);
	                state = S_LENGTH2;
	                break;
	        case S_LENGTH2:
	                /* this is the type */
	                type = c;
	                len--;
	                if (type != MO_NAVDATA) {
	                 /* increase length to cover checksum and tail */
	                 len += 3;
	                 state = S_DISCARD;
	                } else {
	                 memset(&inBuff,0,MAX_PKTLEN);
	                 state = S_COPY;
	                }
	                break;
	        case S_DISCARD:
	                /* keep discarding bytes while we have length to read */
	                len--;
	                if (!len) {
	                 state = S_POSTRESET;
	                }
	                break;
	        case S_COPY:
	                len--;
	                inBuff[n] = c;
	                n++;
	                if (n > MAX_PKTLEN) {
	                 state = S_DISCARD;
	                 //serial0_tx_PGM(PSTR("venus634: overrun\r\n"));
	                 break;
	                }
	                if (!len) {
	                 state = S_ENDCOPY;
	                }
	                break;
	        case S_ENDCOPY:
	                /* collect checksum byte, and compute checksum */
	                if (calcCRC(type, inBuff, n) != c) {
	                 //serial0_tx_PGM(PSTR("venus634: bad checksum\r\n"));
	                 state = S_END;
	                } else {
	                 havepkt = 1;
	                 state = S_END;
	                }
	                break;
	        case S_END:
	                /* just discard */
	                state = S_END1;
	                break;
	        case S_END1:
	                /* discard tail, and go back to look for new packet */
	                state = S_POSTRESET;
	                break;
	  }

	  /* since we'eve processed a character, it might have been the one which
	    completed a packet, check if that's the case */

	  if (havepkt)
		  return type;
	  else return 0xff; //for a failed packet
	}
	return 0x00; //for a regular exit without a packet
}

